import math
from typing import Optional

import numpy as np
from numpy import ndarray


class LineSegment:
    start_point: np.ndarray
    end_point: np.ndarray

    def __init__(self, start_point: np.ndarray, end_point: np.ndarray):
        self.start_point = np.array(start_point)
        self.end_point = np.array(end_point)

    def cal_drop_foot_for(self, point: np.ndarray) -> np.ndarray:
        x0, y0 = point[0], point[1]
        x1, y1 = self.end_point[0], self.end_point[1]
        x2, y2 = self.start_point[0], self.start_point[1]

        k = -((x1 - x0) * (x2 - x1) + (y1 - y0) * (y2 - y1)) / ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2))
        xf = k * (x2 - x1) + x1
        yf = k * (y2 - y1) + y1
        return np.array([xf, yf])

    def cover(self, point: np.ndarray) -> bool:
        return np.dot(point - np.array(self.end_point), point - np.array(self.start_point)) <= 0

    def cal_cross_point_with(self, line) -> Optional[ndarray]:
        """
        :type line: LineSegment
        """
        a0, b0, c0 = self._cal_abc_from_line()
        a1, b1, c1 = line._cal_abc_from_line()
        D = a0 * b1 - a1 * b0

        if D == 0:
            return None
        x = (b0 * c1 - b1 * c0) / D
        y = (a1 * c0 - a0 * c1) / D
        return np.array([x, y])

    def _cal_abc_from_line(self):
        a = self.start_point[1] - self.end_point[1]
        b = self.end_point[0] - self.start_point[0]
        c = self.start_point[0] * self.end_point[1] - self.end_point[0] * self.start_point[1]
        return a, b, c


def cal_angle_between_x(origin: np.ndarray, end_point: np.ndarray):
    arr1 = np.array([1, 0])
    arr2 = end_point - origin
    arr2 = arr2 / np.linalg.norm(arr2)

    return cal_angle_between_a_b(arr1, arr2)


def cal_angle_between_a_b(arr1: np.ndarray, arr2: np.ndarray):
    arr1 = arr1 / np.linalg.norm(arr1)
    arr2 = arr2 / np.linalg.norm(arr2)
    cos = np.dot(arr1, arr2)
    angle = math.acos(cos)

    return angle


def cal_cos_angle_between(arr1: np.ndarray, arr2: np.ndarray):
    arr1 = arr1 / np.linalg.norm(arr1)
    arr2 = arr2 / np.linalg.norm(arr2)
    cos = np.dot(arr1, arr2)

    return cos


def angle_range(start: float, max_value: float, step: float):
    angle_r = [start]
    clockwise, anticlockwise = start, start
    mod = 2 * math.pi
    for i in range(int(max_value / step)):
        clockwise, anticlockwise = (clockwise - step + mod) % mod, (anticlockwise + step) % mod
        angle_r.append(clockwise)
        angle_r.append(anticlockwise)
    return angle_r


def change_angle_range(max_value: float, step: float):
    angle_r = []
    clockwise, anticlockwise = 0.0, 0.0
    mod = 2 * math.pi
    for i in range(int(max_value / step)):
        clockwise, anticlockwise = (clockwise - step + mod) % mod, (anticlockwise + step) % mod
        angle_r.append(clockwise)
        angle_r.append(anticlockwise)
    return angle_r


def transform_angle_to_vector(root: np.ndarray, angle: float) -> np.ndarray:
    return root + np.array([math.cos(angle), math.sin(angle)])


def transform_vector_to_angle(arr: np.ndarray) -> float:
    return cal_angle_between_x(np.array([0, 0]), arr)
